from machine import Pin
from rp2 import PIO,StateMachine,asm_pio
import time
from lib.config.index import sensor_config
from lib.sensors.ssd1306.oled import OLED

_oled = None

def init():
    global _oled, width
    oled_config = sensor_config.get_sensor("ssd1306")

    if oled_config:
        scl = oled_config["scl"]
        sda = oled_config["sda"]
        width = oled_config["width"]
        height = oled_config["height"]
        _oled = OLED(scl, sda, width, height)


init()
main_count=0
pio_count=0

@asm_pio(set_init=rp2.PIO.OUT_LOW)
def blink():
    wrap_target()
    set(pins, 1) [31]
    nop()        [31]
    nop()        [31]
    nop()        [31]
    nop()        [31]
    nop()        [31]
    nop()        [31]
    nop()        [31]
    nop()        [31]
    nop()        [31]
    set(pins, 0) [19]
    nop()        [19]
    wrap()
    
sm1 = StateMachine(0, blink, freq=2000, set_base=Pin(21))
sm2 = StateMachine(1, blink, freq=8000, set_base=Pin(18))
sm1.active(1)
sm2.active(2)


start = time.ticks_us()
max_count = 1000
while True:
    main_count=main_count+1
    if main_count > max_count:
        break
    _oled.clear_rect(0,0,128,10)
    _oled.text(str(main_count), 0,0)
    _oled.show()
end = time.ticks_us()
print(end-start) 